/*
 * LI_Calib: An Open Platform for LiDAR-IMU Calibration
 * Copyright (C) 2020 Jiajun Lv
 * Copyright (C) 2020 Kewei Hu
 * Copyright (C) 2020 Jinhong Xu
 * Copyright (C) 2020 LI_Calib Contributors
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */
#include "imu_processor/estimator_trajectory_manager.h"
#include "utils/math_utils.h"

#include <memory>

template<typename T> 
int kontiki::measurements::LiDARPlaneMeasurement<T>::index = 0;

namespace lio {
using namespace kontiki::trajectories;

void TrajectoryManager::initialTrajTo(double max_time) {
  Eigen::Quaterniond q0 = Eigen::Quaterniond::Identity();
  Eigen::Vector3d p0(0,0,0);
  traj_->R3Spline()->ExtendTo (max_time, p0);
  traj_->SO3Spline()->ExtendTo(max_time, q0);
  std::cout << "spline min time = " << traj_->R3Spline()->MinTime() << "  max time = " << traj_->R3Spline()->MaxTime() << std::endl;
  std::cout << "spline contral point num = " << traj_->R3Spline()->NumKnots() << std::endl;
}

/*
* @param ts te 分别表示新来数据的起始和结束时间
*/
void TrajectoryManager::extendTrajTo(double ts, double te) {
  LOG(INFO) << "extend new control point\n";
  Eigen::Quaterniond q0 = traj_->SO3Spline()->GetEndContralPoint();
  Eigen::Vector3d p0 = traj_->R3Spline()->GetEndContralPoint();
  // Eigen::Quaterniond q0 = Eigen::Quaterniond::Identity();
  // Eigen::Vector3d p0(0,0,0);
  traj_->R3Spline()->ExtendTo (te, p0);
  traj_->SO3Spline()->ExtendTo(te, q0);
  LOG(INFO) << "spline min time = " << traj_->R3Spline()->MinTime() << "  max time = " << traj_->R3Spline()->MaxTime() << std::endl;
  LOG(INFO) << "spline contral point num = " << traj_->R3Spline()->NumKnots() << std::endl;
}

void TrajectoryManager::updateSegments(double ts, double te, int pre_knot) {
  // 固定原有的控制点，只初始化新来的控制点
  int lock_id = activeSeg.fid; // 需要锁定的起始序号
  int lock_num = activeSeg.n + 3;    // 需要锁定的数目
  traj_->R3Spline()->LockContralPoint(lock_id, lock_num);
  traj_->SO3Spline()->LockContralPoint(lock_id, lock_num);
  LOG(INFO) << "lock_id : " << lock_id << "  lock_num : " << lock_num;

  // newest段的起始序号为上一次active段的末尾序号
  newestSeg.fid = activeSeg.fid + activeSeg.n;

  // 更新segments
  int ts_id, te_id;
  double tmp_u, ac_t0;
  traj_->R3Spline()->CalculateIndexAndInterpolationAmount(ts, ts_id, tmp_u);
  traj_->R3Spline()->CalculateIndexAndInterpolationAmount(te, te_id, tmp_u);
  activeSeg.fid = ts_id - pre_knot;
  ac_t0 = activeSeg.fid * traj_->R3Spline()->dt() + traj_->R3Spline()->t0();
  activeSeg.t0 = ac_t0;
  activeSeg.n = te_id - activeSeg.fid + 1;
  activeSeg.dt = traj_->R3Spline()->dt();

  staticSeg.dt = traj_->R3Spline()->dt();
  staticSeg.fid = activeSeg.fid - 3;
  staticSeg.t0 = staticSeg.fid * staticSeg.dt + traj_->R3Spline()->t0();
  staticSeg.n = 3;

  newestSeg.n = te_id - newestSeg.fid + 1;
  newestSeg.dt = traj_->R3Spline()->dt();
  newestSeg.t0 = newestSeg.fid * newestSeg.dt + traj_->R3Spline()->t0();

}

bool TrajectoryManager::deleteContralPointUntill(double t_untill) {
  if (traj_->R3Spline()->DeletePreviousKnot(t_untill) == false) return false;
  if (traj_->SO3Spline()->DeletePreviousKnot(t_untill) == false) return false;
  std::cout << "t_untill = " << t_untill << std::endl;
  std::cout << "spline min time = " << traj_->R3Spline()->MinTime() << "  max time = " << traj_->R3Spline()->MaxTime() << std::endl;
  std::cout << "spline contral point num = " << traj_->R3Spline()->NumKnots() << std::endl;
  return true;
}

void TrajectoryManager::initContralPoint(double t, Eigen::Vector3d &p, Eigen::Quaterniond &q) {
  traj_->R3Spline()->SetContralPoint(t, p);
  traj_->SO3Spline()->SetContralPoint(t, q);
}

void TrajectoryManager::initFirstCP(Eigen::Vector3d &p, Eigen::Quaterniond &q) {
  traj_->R3Spline()->SetFirstContralPoint(p);
  traj_->SO3Spline()->SetFirstContralPoint(q);
}

void TrajectoryManager::initEndCP(Eigen::Vector3d &p, Eigen::Quaterniond &q) {
  traj_->R3Spline()->SetEndContralPoint(p);
  traj_->SO3Spline()->SetEndContralPoint(q);
}

void TrajectoryManager::initNewControlPoint(std::vector<imuPoseAndVel> &pqv,
                                            std::vector<Eigen::Vector3d> &grys,
                                            std::vector<Eigen::Vector3d> &accs) {
  
  lidar_->set_relative_orientation(calib_param_manager->q_LtoI);
  lidar_->set_relative_position(calib_param_manager->p_LinI);
  lidar_->LockRelativeOrientation(true);
  lidar_->LockRelativePosition(true);
  lidar_->LockTimeOffset(true);

  imu_->set_gravity_orientation_pitch(calib_param_manager->gravity_orientation_pitch);
  imu_->set_gravity_orientation_roll(calib_param_manager->gravity_orientation_roll);
  imu_->set_gyroscope_bias(calib_param_manager->gyro_bias);
  imu_->set_accelerometer_bias(calib_param_manager->acce_bias);
  imu_->LockGyroscopeBias(true);
  imu_->LockAccelerometerBias(true);
  imu_->LockGravityPitch(true);
  imu_->LockGravityRoll(true);

  estimator_split = std::make_shared<SplitTrajEstimator>(traj_);
  // 清空之前存储的测量值
  orien_list_.clear();
  position_list_.clear();
  vel_list_.clear();
  gyro_list_.clear();
  accel_list_.clear();
  ba_list_.clear();
  bg_list_.clear();

  double weight_rot = calib_param_manager->global_opt_gyro_weight;
  double weight_pos = calib_param_manager->global_opt_acce_weight;
  // double weight_rot = 1;
  // double weight_pos = 1;
  const double min_time = estimator_split->trajectory()->MinTime();
  const double max_time = estimator_split->trajectory()->MaxTime();
  for (int i = 0; i < pqv.size(); ++i) {
    auto &m = pqv[i];
    double time = m.time;
    if ( min_time > time || max_time <= time) {
      continue;
    }

    auto mg_rot = std::make_shared<OrientationMeasurement>(time, m.rot, weight_rot);
    orien_list_.push_back(mg_rot);

    auto mg_pos = std::make_shared<PositionMeasurement>(time, m.pos, weight_pos);
    position_list_.push_back(mg_pos);

    auto mg_vel = std::make_shared<VelocityMeasurement>(time, m.vel, weight_pos);
    vel_list_.push_back(mg_vel);

    estimator_split->template AddMeasurement<PositionMeasurement>(mg_pos);
    estimator_split->template AddMeasurement<OrientationMeasurement>(mg_rot);
    estimator_split->template AddMeasurement<VelocityMeasurement>(mg_vel);

    // auto mg_gry = std::make_shared<GyroMeasurement>(imu_, time, grys[i], weight_rot);
    // gyro_list_.push_back(mg_gry);
    // estimator_split->template AddMeasurement<GyroMeasurement>(mg_gry);

    // auto mg_acc = std::make_shared<AccelMeasurement>(imu_, time, accs[i], weight_pos);
    // accel_list_.push_back(mg_acc);
    // estimator_split->template AddMeasurement<AccelMeasurement>(mg_acc);
  }

  for (int i = 0; i < 1; ++i) {
    LOG(INFO) << "lock start id: " << traj_->R3Spline()->lock_idx_ << " lock num: " << traj_->R3Spline()->lock_number_;
    ceres::Solver::Summary summary = estimator_split->Solve(50, false);
    SetCalibParam();
    std::cout << summary.BriefReport() << std::endl;
    if (summary.termination_type == ceres::CONVERGENCE) {
      LOG(INFO) << "iter " << i + 1 << " convergence";
      break;
    }
  }

  traj_->R3Spline()->unLockContralPoint();
  traj_->SO3Spline()->unLockContralPoint();
}

void TrajectoryManager::splineSystemInit(std::vector<imuPoseAndVel> &pqv, 
                                         std::vector<Eigen::Vector3d> &grys,
                                         std::vector<Eigen::Vector3d> &accs) {

  lidar_->set_relative_orientation(calib_param_manager->q_LtoI);
  lidar_->set_relative_position(calib_param_manager->p_LinI);
  lidar_->LockRelativeOrientation(true);
  lidar_->LockRelativePosition(true);
  lidar_->LockTimeOffset(true);

  imu_->set_gravity_orientation_pitch(calib_param_manager->gravity_orientation_pitch);
  imu_->set_gravity_orientation_roll(calib_param_manager->gravity_orientation_roll);
  imu_->set_gyroscope_bias(calib_param_manager->gyro_bias);
  imu_->set_accelerometer_bias(calib_param_manager->acce_bias);
  imu_->LockGyroscopeBias(true);
  imu_->LockAccelerometerBias(true);
  imu_->LockGravityPitch(true);
  imu_->LockGravityRoll(true);
  traj_->R3Spline()->unLockContralPoint();
  traj_->SO3Spline()->unLockContralPoint();

  estimator_split = std::make_shared<SplitTrajEstimator>(traj_);
  // 清空之前存储的测量值
  orien_list_.clear();
  position_list_.clear();
  vel_list_.clear();
  gyro_list_.clear();
  accel_list_.clear();
  ba_list_.clear();
  bg_list_.clear();

  double weight_rot = calib_param_manager->global_opt_gyro_weight;
  double weight_pos = calib_param_manager->global_opt_acce_weight;
  const double min_time = estimator_split->trajectory()->MinTime();
  const double max_time = estimator_split->trajectory()->MaxTime();
  for (int i = 0; i < pqv.size(); ++i) {
    auto &m = pqv[i];
    double time = m.time;
    if ( min_time > time || max_time <= time) {
      continue;
    }

    auto mg_rot = std::make_shared<OrientationMeasurement>(time, m.rot, weight_rot);
    orien_list_.push_back(mg_rot);

    auto mg_pos = std::make_shared<PositionMeasurement>(time, m.pos, weight_pos);
    position_list_.push_back(mg_pos);

    auto mg_vel = std::make_shared<VelocityMeasurement>(time, m.vel, weight_pos);
    vel_list_.push_back(mg_vel);

    estimator_split->template AddMeasurement<PositionMeasurement>(mg_pos);
    estimator_split->template AddMeasurement<OrientationMeasurement>(mg_rot);
    estimator_split->template AddMeasurement<VelocityMeasurement>(mg_vel);

    // auto mg_gry = std::make_shared<GyroMeasurement>(imu_, time, grys[i], weight_rot);
    // gyro_list_.push_back(mg_gry);
    // estimator_split->template AddMeasurement<GyroMeasurement>(mg_gry);

    // auto mg_acc = std::make_shared<AccelMeasurement>(imu_, time, accs[i], weight_pos);
    // accel_list_.push_back(mg_acc);
    // estimator_split->template AddMeasurement<AccelMeasurement>(mg_acc);
  }

  for (int i = 0; i < 1; ++i) {
    LOG(INFO) << "lock start id: " << traj_->R3Spline()->lock_idx_ << " lock num: " << traj_->R3Spline()->lock_number_;
    ceres::Solver::Summary summary = estimator_split->Solve(50, false);
    SetCalibParam();
    std::cout << summary.BriefReport() << std::endl;
    if (summary.termination_type == ceres::CONVERGENCE) {
      LOG(INFO) << "iter " << i + 1 << " convergence";
      break;
    }
  }
}

void TrajectoryManager::trajInit() {
  lidar_->set_relative_orientation(calib_param_manager->q_LtoI);
  lidar_->set_relative_position(calib_param_manager->p_LinI);
  lidar_->LockRelativeOrientation(true);
  lidar_->LockRelativePosition(true);
  lidar_->LockTimeOffset(true);

  imu_->set_gravity_orientation_pitch(calib_param_manager->gravity_orientation_pitch);
  imu_->set_gravity_orientation_roll(calib_param_manager->gravity_orientation_roll);
  imu_->set_gyroscope_bias(calib_param_manager->gyro_bias);
  imu_->set_accelerometer_bias(calib_param_manager->acce_bias);
  imu_->LockGyroscopeBias(false);
  imu_->LockAccelerometerBias(false);
  imu_->LockGravityPitch(true);
  imu_->LockGravityRoll(true);

  estimator_split = std::make_shared<SplitTrajEstimator>(traj_);
  // 清空之前存储的测量值
  edge_list_.clear();
  plane_list_.clear();
  pivotPlane_list_.clear();
  gyro_list_.clear();
  accel_list_.clear();
  ba_list_.clear();
  bg_list_.clear();
  vel_list_.clear();
  relative_vel_list_.clear();

  // 固定 static 控制点
  int lock_id = staticSeg.fid;
  traj_->R3Spline()->LockContralPoint(lock_id, 3);
  traj_->SO3Spline()->LockContralPoint(lock_id, 3);
}

void TrajectoryManager::trajInitForNewContralPoint() {
  imu_->LockGyroscopeBias(true);
  imu_->LockAccelerometerBias(true);
  imu_->LockGravityPitch(true);
  imu_->LockGravityRoll(true);

  estimator_split = std::make_shared<SplitTrajEstimator>(traj_);
  // 清空之前存储的测量值
  gyro_list_.clear();
  accel_list_.clear();
}

void TrajectoryManager::solveOptProblem(bool &is_convergence) {
  // addCallback(estimator_split);
  //printErrorStatistics("Before optimization");
  LOG(INFO) << "lock start id: " << traj_->R3Spline()->lock_idx_ << " lock num: " << traj_->R3Spline()->lock_number_;
  ceres::Solver::Summary summary = estimator_split->Solve(50, false);
  // std::cout << summary.BriefReport() << std::endl;
  std::cout << summary.FullReport() << std::endl;
  is_convergence = summary.termination_type == ceres::CONVERGENCE;
  // printErrorStatistics("After optimization");
  traj_->R3Spline()->unLockContralPoint();
  traj_->SO3Spline()->unLockContralPoint();

}

void TrajectoryManager::SetCalibParam() {
  calib_param_manager->set_p_LinI(lidar_->relative_position());
  calib_param_manager->set_q_LtoI(lidar_->relative_orientation());
  calib_param_manager->set_time_offset(lidar_->time_offset());
  calib_param_manager->set_gravity(imu_->refined_gravity());
  calib_param_manager->set_gyro_bias(imu_->gyroscope_bias());
  calib_param_manager->set_acce_bias(imu_->accelerometer_bias());
  calib_param_manager->set_gravity_orientation_roll(imu_->gravity_orientation_roll());
  calib_param_manager->set_gravity_orientation_pitch(imu_->gravity_orientation_pitch());
}

void TrajectoryManager::ShowCalibParam() {
  calib_param_manager->showStates();
}

double TrajectoryManager::evaluateResidual() {
  return estimator_split->evaluateResidual();
}

double TrajectoryManager::evaluateResidual(std::vector<ceres::internal::ResidualBlock *> &res) {
  return estimator_split->evaluateResidual(res);
}

void TrajectoryManager::clearResidual() {
  estimator_split->clearResidual();
}

std::vector<ceres::internal::ResidualBlock *>& TrajectoryManager::getResidualIds() {
  return estimator_split->res_ids;
}

bool TrajectoryManager::evaluateLidarPose(double lidar_time, Eigen::Vector3d& p_LinG, Eigen::Quaterniond& q_LtoG) const {
  if (traj_->MinTime() > lidar_time || traj_->MaxTime() <= lidar_time)
    return false;
  Result result = traj_->Evaluate(lidar_time, EvalOrientation | EvalPosition);
  q_LtoG = result->orientation * calib_param_manager->q_LtoI;
  p_LinG = result->orientation * calib_param_manager->p_LinI + result->position;
  return true;
}

void TrajectoryManager::addEdgeMeasurement(Eigen::Vector3d &curr_point_, Eigen::Vector3d &last_point_a_,
					                Eigen::Vector3d &last_point_b_, const double frame_time, double distance_weight){
  const double weight = calib_param_manager->global_opt_lidar_weight * distance_weight;
  auto msp = std::make_shared<EdgeMeasurement> (lidar_, curr_point_, last_point_a_, last_point_b_, frame_time, 1.0, weight);
  // surfelpoint_list_.push_back(msp);
  edge_list_.push_back(msp);
  estimator_split->template AddMeasurement<EdgeMeasurement>(msp);

}

void TrajectoryManager::addPlaneMeasurement(Eigen::Vector3d &curr_point_, Eigen::Vector3d &plane_unit_norm_,
						               double d_, const double frame_time){
  const double weight = calib_param_manager->global_opt_lidar_weight;
  auto msp = std::make_shared<PlaneMeasurement> (lidar_, curr_point_, plane_unit_norm_, d_, frame_time, 1.0, weight);
  // surfelpoint_list_.push_back(msp);
  plane_list_.push_back(msp);
  estimator_split->template AddMeasurement<PlaneMeasurement>(msp);

}

// void TrajectoryManager::addPivotPointPlaneMeasurement(Eigen::Vector3d &curr_point_, Eigen::Vector4d &coeff_,
// 						               double pivot_time_, const double frame_time){
//   const double weight = calib_param_manager->global_opt_lidar_weight;
//   auto msp = std::make_shared<PivotPointPlaneMeasurement> (lidar_, curr_point_, coeff_, pivot_time_, frame_time, 1.0, weight);
//   // surfelpoint_list_.push_back(msp);
//   pivotPlane_list_.push_back(msp);
//   estimator_split->template AddMeasurement<PivotPointPlaneMeasurement>(msp);

// }

void TrajectoryManager::addPivotPointPlaneMeasurement(Eigen::Vector3d &curr_point_, Eigen::Vector4d &coeff_,
						               Eigen::Vector3d &pivot_pos_, Eigen::Quaterniond &pivot_rot_, const double frame_time) {
  const double weight = calib_param_manager->global_opt_lidar_weight;
  auto msp = std::make_shared<PivotPointPlaneMeasurement> (lidar_, curr_point_, coeff_, pivot_pos_, pivot_rot_, frame_time, 1.0, weight);
  // surfelpoint_list_.push_back(msp);
  pivotPlane_list_.push_back(msp);
  estimator_split->template AddMeasurement<PivotPointPlaneMeasurement>(msp);
}

void TrajectoryManager::addGyroscopeMeasurements(std::vector<Eigen::Vector3d> &grys, std::vector<double> &times) {
  int grys_size = grys.size();
  int times_size = times.size();
  if (grys_size != times_size) {
    LOG(ERROR) << "grys_size != times_size";
    return;
  }

  double weight = calib_param_manager->global_opt_gyro_weight;
  const double min_time = estimator_split->trajectory()->MinTime();
  const double max_time = estimator_split->trajectory()->MaxTime();
  for (int i = 0; i < grys_size; ++i) {
    Eigen::Vector3d &gry = grys[i];
    double time = times[i];
    if ( min_time > time || max_time <= time) {
      continue;
    }

    auto mg = std::make_shared<GyroMeasurement>(imu_, time, gry, weight);
    gyro_list_.push_back(mg);
    estimator_split->template AddMeasurement<GyroMeasurement>(mg);
  }
}

void TrajectoryManager::addAccelerometerMeasurement(std::vector<Eigen::Vector3d> &accls, std::vector<double> &times) {
  int accls_size = accls.size();
  int times_size = times.size();
  if (accls_size != times_size) {
    LOG(ERROR) << "accls_size != times_size";
    return;
  }

  double weight = calib_param_manager->global_opt_acce_weight;
  const double min_time = estimator_split->trajectory()->MinTime();
  const double max_time = estimator_split->trajectory()->MaxTime();
  for (int i = 0; i < accls_size; ++i) {
    Eigen::Vector3d &accl = accls[i];
    double time = times[i];
    if ( min_time > time || max_time <= time) {
      continue;
    }

    auto mg = std::make_shared<AccelMeasurement>(imu_, time, accl, weight);
    accel_list_.push_back(mg);
    estimator_split->template AddMeasurement<AccelMeasurement>(mg);
  }
}

void TrajectoryManager::addLinearVelocityMeasurement(std::vector<Eigen::Vector3d> &accls, std::vector<double> &times) {
  int accls_size = accls.size();
  int times_size = times.size();
  if (accls_size != times_size) {
    LOG(ERROR) << "accls_size != times_size";
    return;
  }

  double weight = calib_param_manager->global_opt_gyro_weight;
  const double min_time = estimator_split->trajectory()->MinTime();
  const double max_time = estimator_split->trajectory()->MaxTime();
  for (int i = 0; i < accls_size - 1; ++i) {
    Eigen::Vector3d &accli = accls[i];
    Eigen::Vector3d &acclj = accls[i + 1];
    double timei = times[i];
    double timej = times[i + 1];
    if (min_time > timei || max_time <= timei || min_time > timej || max_time <= timej) {
      continue;
    }

    // 这里给的 accls 是按时间从后向前排列的, 所以times[i] > times[i+1]
    // auto mg = std::make_shared<RelativeVelocityMeasurement>(imu_, timej, timei, acclj, accli, weight);
    // relative_vel_list_.push_back(mg);
    // estimator_split->template AddMeasurement<RelativeVelocityMeasurement>(mg);

    // auto mg_vel = std::make_shared<VelocityMeasurement>(times[i], Eigen::Vector3d::Zero(), weight);
    // vel_list_.push_back(mg_vel);
    // estimator_split->template AddMeasurement<VelocityMeasurement>(mg_vel);

    // auto mg = std::make_shared<RelativeVelocityMeasurement>(imu_, times.back(), times[i], accls.back(), accls[i], weight);
    // relative_vel_list_.push_back(mg);
    // estimator_split->template AddMeasurement<RelativeVelocityMeasurement>(mg);
  }
  // auto mg = std::make_shared<RelativeVelocityMeasurement>(imu_, times.back(), times[0], accls.back(), accls[0], weight);
  // relative_vel_list_.push_back(mg);
  // estimator_split->template AddMeasurement<RelativeVelocityMeasurement>(mg);

  // auto mg_vel = std::make_shared<VelocityMeasurement>(times.back(), Eigen::Vector3d::Zero(), weight);
  // vel_list_.push_back(mg_vel);
  // estimator_split->template AddMeasurement<VelocityMeasurement>(mg_vel);
}

void TrajectoryManager::addAcceAndGrysBiasMeasurement(Eigen::Vector3d &ba, Eigen::Vector3d &bg, double weight_a, double weight_g) {
  auto mg_ba = std::make_shared<AcceBiasMeasurement>(imu_, ba, weight_a);
  ba_list_.push_back(mg_ba);
  estimator_split->template AddMeasurement<AcceBiasMeasurement>(mg_ba);

  auto mg_bg = std::make_shared<GrysBiasMeasurement>(imu_, bg, weight_g);
  bg_list_.push_back(mg_bg);
  estimator_split->template AddMeasurement<GrysBiasMeasurement>(mg_bg);
}

void TrajectoryManager::addInitQuaternionMeasurement() {
  double t0 = traj_->SO3Spline()->MinTime();
  double weight = calib_param_manager->global_opt_gyro_weight;
  Eigen::AngleAxisd rotation_vector(0.0001, Eigen::Vector3d(0,0,1));
  Eigen::Quaterniond q0 = Eigen::Quaterniond (rotation_vector.matrix());
  m_q0_ = std::make_shared<OrientationMeasurement>(t0, q0, weight);
  estimator_split->AddMeasurement<OrientationMeasurement>(m_q0_);

  Eigen::Vector3d p0(0, 0, 0);
  weight = calib_param_manager->global_opt_acce_weight;
  m_p0_= std::make_shared<PositionMeasurement>(t0, p0, weight);
  estimator_split->AddMeasurement<PositionMeasurement>(m_p0_);
}

bool TrajectoryManager::evaluateLidarRelativePose(double lidar_time1,
        double lidar_time2, Eigen::Quaterniond &q_L2toL1, Eigen::Vector3d &p_L2inL1) const {

  double traj_time1 = lidar_time1;
  double traj_time2 = lidar_time2;

  if (traj_->MinTime() > traj_time1 || traj_->MaxTime() <= traj_time2)
    return false;

  Result result1 = traj_->Evaluate(traj_time1, EvalOrientation | EvalPosition);
  Result result2 = traj_->Evaluate(traj_time2, EvalOrientation | EvalPosition);
  Eigen::Quaterniond q_I2toI1 = result1->orientation.conjugate()*result2->orientation;

  q_L2toL1 = calib_param_manager->q_LtoI.conjugate() * q_I2toI1 * calib_param_manager->q_LtoI;  
  
  p_L2inL1 = calib_param_manager->q_LtoI.conjugate() * q_I2toI1 * calib_param_manager->p_LinI
           + calib_param_manager->q_LtoI.conjugate() * result1->orientation.conjugate() 
           * (result2->position - result1->position)
           - calib_param_manager->q_LtoI.conjugate() * calib_param_manager->p_LinI;

  return true;
}

Eigen::Quaterniond TrajectoryManager::evaluateRotateAlignGravity() const{
  double cosRoll = cos(calib_param_manager->gravity_orientation_roll);
  double sinRoll = sin(calib_param_manager->gravity_orientation_roll);
  double cosPitch = cos(calib_param_manager->gravity_orientation_pitch);
  double sinPitch = sin(calib_param_manager->gravity_orientation_pitch);

  Eigen::Matrix3d Rr;
  Rr << 1., 0., 0.,
      0., cosRoll, sinRoll,
      0., -sinRoll, cosRoll;
  
  Eigen::Matrix3d Rp;
  Rp << cosPitch, 0., -sinPitch,
      0., 1., 0.,
      sinPitch, 0., cosPitch;

  return Eigen::Quaterniond(Rr * Rp);
}

bool TrajectoryManager::evaluateLidarPoseAlignGravity(double lidar_time, 
                              Eigen::Vector3d& p, Eigen::Quaterniond& q) const {
  if (evaluateLidarPose(lidar_time, p, q)) {
    Eigen::Quaterniond C_w_I0 = evaluateRotateAlignGravity();
    p = C_w_I0 * p;
    q = C_w_I0 * q;
    return true;
  } else {
    return false;
  }
}

bool TrajectoryManager::evaluateImuVelocityAlignGravity(double t, Eigen::Vector3d &v) const {
  if (traj_->MinTime() > t || traj_->MaxTime() <= t)
    return false;
  Result result = traj_->Evaluate(t, EvalVelocity);
  Eigen::Quaterniond C_w_I0 = evaluateRotateAlignGravity();
  v = C_w_I0 * result->velocity;
  return true;
}

bool TrajectoryManager::evaluateImuPoseAlignGravity(double t, Eigen::Vector3d& p, Eigen::Quaterniond& q) const {
  if (traj_->MinTime() > t || traj_->MaxTime() <= t)
    return false;
  Result result = traj_->Evaluate(t, EvalOrientation | EvalPosition);
  Eigen::Quaterniond C_w_I0 = evaluateRotateAlignGravity();
  p = C_w_I0 * result->position;
  q = C_w_I0 * result->orientation;
  return true;
}

bool TrajectoryManager::getExtrinsic(Eigen::Vector3d& p, Eigen::Quaterniond& q) const {
  p = calib_param_manager->p_LinI;
  q = calib_param_manager->q_LtoI;
  return true;
}

bool TrajectoryManager::evaluateImuPose(double t, Eigen::Vector3d& p, Eigen::Quaterniond& q) const {
  if (traj_->MinTime() > t || traj_->MaxTime() <= t)
    return false;
  Result result = traj_->Evaluate(t, EvalOrientation | EvalPosition);
  p = result->position;
  q = result->orientation;
  return true;
}

bool TrajectoryManager::evaluateImuVelocity(double t, Eigen::Vector3d &v) const {
  if (traj_->MinTime() > t || traj_->MaxTime() <= t)
    return false;
  Result result = traj_->Evaluate(t, EvalVelocity);
  v = result->velocity;
  return true;
}

bool TrajectoryManager::eavluateImuAcceleration(double t, Eigen::Vector3d &a) const {
  if (traj_->MinTime() > t || traj_->MaxTime() <= t)
    return false;
  Result result = traj_->Evaluate(t, EvalOrientation | EvalAcceleration);
  a = result->orientation.conjugate() * (result->acceleration - imu_->refined_gravity()) + imu_->accelerometer_bias();
  return true;
}

bool TrajectoryManager::eavluateImuGryscope(double t, Eigen::Vector3d &w) const {
  if (traj_->MinTime() > t || traj_->MaxTime() <= t)
    return false;
  Result result = traj_->Evaluate(t, EvalOrientation | EvalAngularVelocity);
  // Rotate from world to body coordinate frame
  w = result->orientation.conjugate() * result->angular_velocity + imu_->gyroscope_bias();
  return true;
}

void TrajectoryManager::printErrorStatistics(const std::string& intro, bool show_gyro,
                                             bool show_accel, bool show_lidar) const {
  std::cout << "\n============== " << intro << " ================" << std::endl;

  if (show_gyro && !gyro_list_.empty()) {
    Eigen::Vector3d error_sum(0, 0, 0), err(0, 0, 0);
    double sqsum = 0.0;
    for(auto const& m : gyro_list_) {
      err = m->ErrorRaw<SplitTrajectory> (*traj_);
      error_sum += err.cwiseAbs();
      sqsum += err.squaredNorm();
    }
    std::cout << "[Gyro]  Error size, average, squared sum: " << gyro_list_.size()
              << "; " << (error_sum/gyro_list_.size()).transpose() << "   " << sqsum << std::endl;
  }

  if (show_accel && !accel_list_.empty()) {
    Eigen::Vector3d error_sum(0, 0, 0), err(0, 0, 0);
    double sqsum = 0.0;
    for(auto const& m : accel_list_) {
      err = m->ErrorRaw<SplitTrajectory> (*traj_);
      error_sum += err.cwiseAbs();
      sqsum += err.squaredNorm();
    }
    std::cout << "[Accel] Error size, average, squared sum: " << accel_list_.size()
              << ";  " << (error_sum/accel_list_.size()).transpose() << "   " << sqsum << std::endl;
  }

  if (show_accel && !relative_vel_list_.empty()) {
    Eigen::Vector3d error_sum(0, 0, 0), err(0, 0, 0);
    double sqsum = 0.0;
    for(auto const& m : relative_vel_list_) {
      err = m->ErrorRaw<SplitTrajectory> (*traj_);
      error_sum += err.cwiseAbs();
      sqsum += err.squaredNorm();
    }
    std::cout << "[Relative vel] Error size, average, squared sum: " << relative_vel_list_.size()
              << ";  " << (error_sum/relative_vel_list_.size()).transpose() << "   " << sqsum << std::endl;
  }

  if (show_lidar && !plane_list_.empty()) {
    Eigen::Matrix<double,1,1>  error_sum(0), err(0);
    double sqsum = 0.0;
    for (auto const &m : plane_list_) {
      err = m->point2plane<SplitTrajectory> (*traj_);
      error_sum += err.cwiseAbs();
      sqsum += err.squaredNorm();
    }
    std::cout << "[Surface] Error size, average, squared sum: " << plane_list_.size()
              << "; " << (error_sum/plane_list_.size()).transpose() << "   " << sqsum << std::endl;
  }

  if (show_lidar && !edge_list_.empty()) {
    Eigen::Matrix<double,3,1>  error_sum(0, 0, 0), err(0, 0, 0);
    double sqsum = 0.0;
    for (auto const &m : edge_list_) {
      err = m->Point2Line<SplitTrajectory> (*traj_);
      error_sum += err.cwiseAbs();
      sqsum += err.squaredNorm();
    }
    std::cout << "[Corner] Error size, average, squared sum: " << edge_list_.size()
              << "; " << (error_sum/edge_list_.size()).transpose() << "   " << sqsum << std::endl;
  }

  if (!ba_list_.empty()) {
    Eigen::Vector3d  error_sum(0, 0, 0), err(0, 0, 0);
    double sqsum = 0.0;
    for (auto const &m : ba_list_) {
      err = m->ErrorRaw();
      error_sum += err.cwiseAbs();
      sqsum += err.squaredNorm();
    }
    std::cout << "[Bias acc] Error size, average, squared sum: " << ba_list_.size()
              << "; " << (error_sum/ba_list_.size()).transpose() << "   " << sqsum << std::endl;
  }

  if (!bg_list_.empty()) {
    Eigen::Vector3d  error_sum(0, 0, 0), err(0, 0, 0);
    double sqsum = 0.0;
    for (auto const &m : bg_list_) {
      err = m->ErrorRaw();
      error_sum += err.cwiseAbs();
      sqsum += err.squaredNorm();
    }
    std::cout << "[Bias gry] Error size, average, squared sum: " << bg_list_.size()
              << "; " << (error_sum/bg_list_.size()).transpose() << "   " << sqsum << std::endl;
  }

  std::cout << std::endl;
}

void TrajectoryManager::initControlPointToGlobal() {
  Eigen::Quaterniond C_w_I0 = evaluateRotateAlignGravity();
  for (int i = 0; i < traj_->R3Spline()->NumKnots(); ++i) {
    // T^G_Im = R^G_I0 * T^I0_Im
    Eigen::Quaterniond q_ImtoG = C_w_I0 * traj_->SO3Spline()->GetContralPointaAt(i);
    Eigen::Vector3d p_IminG = C_w_I0 * traj_->R3Spline()->GetContralPointaAt(i);
    traj_->SO3Spline()->SetContralPointAt(i, q_ImtoG);
    traj_->R3Spline()->SetContralPointAt(i, p_IminG);
  }
  calib_param_manager->set_gravity_orientation_pitch(0);
  calib_param_manager->set_gravity_orientation_roll(0);
  Eigen::Vector3d g_w = Eigen::Vector3d(0, 0, -1) * kontiki::sensors::internal::STANDARD_GRAVITY;
  calib_param_manager->set_gravity(g_w);
}


}